/*
  ADCS testing program loop
*/

#include <msp430x16x.h>
#include <signal.h>

#include "main.h"

#include "log/log.h"
#include "timing/timing.h"
#include "recovery/recover.h"
#include "i2c/i2c.h"
#include "sd/sd.h"
#include "payload/pay.h"
#include "coms/coms.h"
#include "adcs/adcs.h"
#include "adc/adc.h"

int main() {
  int modem;
  int attitude[3];
  int i;
  int test;
  unsigned char inputState, inputPin;

  /* EINT BREAKS INTERRUPTS ON ADC */
  //eint(); // enable interrupts 
  
  P6DIR = 0;
  P5DIR = 0xFF;
  P2DIR = 0xFF;
  
  // Initialisations
  //sdInit();
  //comsInit();
  adcInit();
  
  inputPin = 0x80; // change to choose which pin to use on P6
  
  inputState = inputPin;
  
  P2OUT = 0x00;
  while (1) {   
    //if (!(P6IN ^ inputState)) { // check for a changed value of input pin
    //  if (P6IN && inputPin) { // only run if the value has gone from low to high
        //ADCS(attitude);
        
		test = adcSample(6,1);
		while(1) {
			P2OUT = test;
			for (i=0; i<10000; i++); //wait
			for (i=0; i<10000; i++); //wait
			for (i=0; i<10000; i++); //wait
			for (i=0; i<10000; i++); //wait
			P2OUT = test/256;
			for (i=0; i<10000; i++); //wait
			for (i=0; i<10000; i++); //wait
			for (i=0; i<10000; i++); //wait
			for (i=0; i<10000; i++); //wait
		}
		
        //modTransmit((unsigned char)attitude[0]);
        //modTransmit((unsigned char)attitude[1]);
        //modTransmit((unsigned char)attitude[2]);
		/*
		P2OUT = 0xFF;
		for (i=0; i<10000; i++); //wait
		P2OUT = 0x00;
		for (i=0; i<10000; i++); //wait
		*/
		
		/*
		P2OUT = attitude[0];
		for (i=0; i<10000; i++); //wait
		P2OUT = attitude[0]/256;
		for (i=0; i<10000; i++); //wait
		P2OUT = attitude[1];
		for (i=0; i<10000; i++); //wait
		P2OUT = attitude[1]/256;
		for (i=0; i<10000; i++); //wait
		P2OUT = attitude[2];
		for (i=0; i<10000; i++); //wait
		P2OUT = attitude[2]/256;
		for (i=0; i<10000; i++); //wait
		
		P2OUT = 0xFF;
		for (i=0; i<20000; i++); //wait
		
        inputState = 0x00;
		*/
    //  }
    //  else {
    //    inputState = inputPin;
    //  }
    //}
  }
}
